#include <math.h>
#include <string.h>
#include "pm_std.h"
#include "sm_std.h"
#include "ne_std.h"
#include "ne_dae.h"
#include "sm_ssci_run_time_errors.h"
#include "sm_RuntimeDerivedValuesBundle.h"
#include "openManipulatorTrajectoryPlanning_6a081443_1_geometries.h"
PmfMessageId openManipulatorTrajectoryPlanning_6a081443_1_recordLog ( const
RuntimeDerivedValuesBundle * rtdv , const int * eqnEnableFlags , const double
* state , const int * modeVector , const double * input , const double *
inputDot , const double * inputDdot , double * logVector , double *
errorResult , NeuDiagnosticManager * neDiagMgr ) { const double * rtdvd =
rtdv -> mDoubles . mValues ; const int * rtdvi = rtdv -> mInts . mValues ;
int ii [ 1 ] ; double xx [ 200 ] ; ( void ) rtdvd ; ( void ) rtdvi ; ( void )
eqnEnableFlags ; ( void ) modeVector ; ( void ) inputDot ; ( void ) inputDdot
; ( void ) neDiagMgr ; xx [ 0 ] = 57.29577951308232 ; xx [ 1 ] = 0.2 ; xx [ 2
] = 4.385038631106116e-3 ; xx [ 3 ] = 0.01057602111633074 ; xx [ 4 ] =
0.999934457458863 ; xx [ 5 ] = xx [ 2 ] ; xx [ 6 ] = - xx [ 3 ] ; xx [ 7 ] =
xx [ 4 ] ; xx [ 8 ] = xx [ 2 ] * state [ 1 ] ; xx [ 9 ] = - ( xx [ 3 ] *
state [ 1 ] ) ; xx [ 10 ] = xx [ 4 ] * state [ 1 ] ; xx [ 11 ] =
1.122487308813095e-6 * state [ 1 ] ; xx [ 12 ] = - ( 2.687675606235184e-6 *
state [ 1 ] ) ; xx [ 13 ] = 1.885591678654433e-5 * state [ 1 ] ;
pm_math_Vector3_cross_ra ( xx + 8 , xx + 11 , xx + 14 ) ; xx [ 17 ] = -
0.9984344664497182 ; xx [ 18 ] = - 2.745981335367077e-3 ; xx [ 19 ] = -
0.05574004796882191 ; xx [ 20 ] = 3.758037281069619e-3 ; xx [ 11 ] = 0.5 ; xx
[ 12 ] = xx [ 11 ] * state [ 2 ] ; xx [ 13 ] = 0.01415235514121426 ; xx [ 21
] = sin ( xx [ 12 ] ) ; xx [ 22 ] = 0.9998919020591995 ; xx [ 23 ] =
3.986858461632603e-3 ; xx [ 24 ] = cos ( xx [ 12 ] ) ; xx [ 25 ] = - ( xx [
13 ] * xx [ 21 ] ) ; xx [ 26 ] = xx [ 22 ] * xx [ 21 ] ; xx [ 27 ] = xx [ 23
] * xx [ 21 ] ; pm_math_Quaternion_compose_ra ( xx + 17 , xx + 24 , xx + 28 )
; pm_math_Quaternion_inverseXform_ra ( xx + 28 , xx + 8 , xx + 17 ) ; xx [ 12
] = xx [ 13 ] * state [ 3 ] ; xx [ 20 ] = xx [ 17 ] - xx [ 12 ] ; xx [ 21 ] =
xx [ 22 ] * state [ 3 ] ; xx [ 24 ] = xx [ 18 ] + xx [ 21 ] ; xx [ 25 ] = xx
[ 23 ] * state [ 3 ] ; xx [ 26 ] = xx [ 19 ] + xx [ 25 ] ; xx [ 32 ] = xx [
20 ] ; xx [ 33 ] = xx [ 24 ] ; xx [ 34 ] = xx [ 26 ] ; xx [ 27 ] =
1.782974915729205e-3 ; xx [ 35 ] = 1.790292518937343e-3 ; xx [ 36 ] =
5.529481718271925e-5 ; xx [ 37 ] = xx [ 20 ] * xx [ 27 ] ; xx [ 38 ] = xx [
24 ] * xx [ 35 ] ; xx [ 39 ] = xx [ 26 ] * xx [ 36 ] ;
pm_math_Vector3_cross_ra ( xx + 32 , xx + 37 , xx + 40 ) ; xx [ 43 ] = -
0.9984606559510578 ; xx [ 44 ] = - 1.525595350473025e-3 ; xx [ 45 ] =
0.05467563387767577 ; xx [ 46 ] = - 9.195984813180454e-3 ; xx [ 20 ] = xx [
11 ] * state [ 4 ] ; xx [ 24 ] = 4.564927305666077e-3 ; xx [ 26 ] = sin ( xx
[ 20 ] ) ; xx [ 37 ] = 0.9999885204623049 ; xx [ 38 ] = 1.456153255698589e-3
; xx [ 47 ] = cos ( xx [ 20 ] ) ; xx [ 48 ] = xx [ 24 ] * xx [ 26 ] ; xx [ 49
] = xx [ 37 ] * xx [ 26 ] ; xx [ 50 ] = xx [ 38 ] * xx [ 26 ] ;
pm_math_Quaternion_compose_ra ( xx + 43 , xx + 47 , xx + 51 ) ;
pm_math_Quaternion_inverseXform_ra ( xx + 51 , xx + 32 , xx + 43 ) ; xx [ 20
] = xx [ 24 ] * state [ 5 ] ; xx [ 26 ] = xx [ 43 ] + xx [ 20 ] ; xx [ 39 ] =
xx [ 37 ] * state [ 5 ] ; xx [ 46 ] = xx [ 44 ] + xx [ 39 ] ; xx [ 47 ] = xx
[ 38 ] * state [ 5 ] ; xx [ 48 ] = xx [ 45 ] + xx [ 47 ] ; xx [ 55 ] = xx [
26 ] ; xx [ 56 ] = xx [ 46 ] ; xx [ 57 ] = xx [ 48 ] ; xx [ 49 ] =
3.064698286834481e-5 ; xx [ 50 ] = 1.339420127444153e-3 ; xx [ 58 ] =
1.348660701873222e-3 ; xx [ 59 ] = xx [ 26 ] * xx [ 49 ] ; xx [ 60 ] = xx [
46 ] * xx [ 50 ] ; xx [ 61 ] = xx [ 48 ] * xx [ 58 ] ;
pm_math_Vector3_cross_ra ( xx + 55 , xx + 59 , xx + 62 ) ; xx [ 65 ] = -
0.9953936295809525 ; xx [ 66 ] = - 5.058992580434195e-4 ; xx [ 67 ] =
0.09584258966610114 ; xx [ 68 ] = 2.337576029516223e-3 ; xx [ 26 ] = xx [ 11
] * state [ 6 ] ; xx [ 46 ] = 8.310758752905212e-6 ; xx [ 48 ] = sin ( xx [
26 ] ) ; xx [ 59 ] = 0.9999999999652471 ; xx [ 60 ] = 6.610584974975353e-7 ;
xx [ 69 ] = cos ( xx [ 26 ] ) ; xx [ 70 ] = xx [ 46 ] * xx [ 48 ] ; xx [ 71 ]
= xx [ 59 ] * xx [ 48 ] ; xx [ 72 ] = - ( xx [ 60 ] * xx [ 48 ] ) ;
pm_math_Quaternion_compose_ra ( xx + 65 , xx + 69 , xx + 73 ) ;
pm_math_Quaternion_inverseXform_ra ( xx + 73 , xx + 55 , xx + 65 ) ; xx [ 26
] = xx [ 46 ] * state [ 7 ] ; xx [ 48 ] = xx [ 65 ] + xx [ 26 ] ; xx [ 61 ] =
xx [ 59 ] * state [ 7 ] ; xx [ 68 ] = xx [ 66 ] + xx [ 61 ] ; xx [ 69 ] = xx
[ 60 ] * state [ 7 ] ; xx [ 70 ] = xx [ 67 ] - xx [ 69 ] ; xx [ 77 ] = xx [
48 ] ; xx [ 78 ] = xx [ 68 ] ; xx [ 79 ] = xx [ 70 ] ; xx [ 80 ] = xx [ 48 ]
* 8.094917776298291e-5 ; xx [ 81 ] = xx [ 68 ] * 3.67360393682157e-4 ; xx [
82 ] = xx [ 70 ] * 3.844288509185152e-4 ; pm_math_Vector3_cross_ra ( xx + 77
, xx + 80 , xx + 70 ) ; xx [ 48 ] = 0.05259203501857801 ; xx [ 68 ] =
4.439189670580984e-7 ; xx [ 80 ] = 0.01034590993618254 ; xx [ 81 ] = xx [ 48
] ; xx [ 82 ] = - xx [ 68 ] ; xx [ 83 ] = - xx [ 80 ] ; xx [ 84 ] =
0.7037746403582225 ; xx [ 85 ] = 0.06856918658830206 ; xx [ 86 ] = xx [ 85 ]
* input [ 6 ] ; xx [ 87 ] = 0.06856329236118894 ; xx [ 88 ] =
0.7037745357569194 ; xx [ 89 ] = xx [ 88 ] * input [ 6 ] ; xx [ 90 ] = 2.0 ;
xx [ 91 ] = ( xx [ 84 ] * xx [ 86 ] - xx [ 87 ] * xx [ 89 ] ) * xx [ 90 ] ;
xx [ 92 ] = xx [ 90 ] * ( xx [ 87 ] * xx [ 86 ] + xx [ 84 ] * xx [ 89 ] ) ;
xx [ 84 ] = input [ 6 ] - ( xx [ 88 ] * xx [ 89 ] + xx [ 85 ] * xx [ 86 ] ) *
xx [ 90 ] ; xx [ 85 ] = xx [ 91 ] ; xx [ 86 ] = xx [ 92 ] ; xx [ 87 ] = xx [
84 ] ; pm_math_Vector3_cross_ra ( xx + 81 , xx + 85 , xx + 93 ) ; xx [ 85 ] =
0.7037745357569191 ; xx [ 86 ] = 0.0685632923611889 ; xx [ 87 ] = xx [ 86 ] *
input [ 8 ] ; xx [ 88 ] = 0.06856918658830211 ; xx [ 89 ] =
0.7037746403582228 ; xx [ 96 ] = xx [ 89 ] * input [ 8 ] ; xx [ 97 ] = ( xx [
85 ] * xx [ 87 ] - xx [ 88 ] * xx [ 96 ] ) * xx [ 90 ] ; xx [ 98 ] = xx [ 90
] * ( xx [ 88 ] * xx [ 87 ] + xx [ 85 ] * xx [ 96 ] ) ; xx [ 85 ] = input [ 8
] - ( xx [ 89 ] * xx [ 96 ] + xx [ 86 ] * xx [ 87 ] ) * xx [ 90 ] ; xx [ 86 ]
= xx [ 97 ] ; xx [ 87 ] = - xx [ 98 ] ; xx [ 88 ] = xx [ 85 ] ;
pm_math_Vector3_cross_ra ( xx + 81 , xx + 86 , xx + 99 ) ; xx [ 81 ] = -
0.9952875672845549 ; xx [ 82 ] = - 0.09696730578610657 ; xx [ 86 ] = xx [ 81
] ; xx [ 87 ] = - 7.396429069217447e-8 ; xx [ 88 ] = xx [ 82 ] ; xx [ 89 ] =
4.167847961547566e-6 ; pm_math_Quaternion_inverseXform_ra ( xx + 86 , xx + 77
, xx + 102 ) ; xx [ 83 ] = 1.0e-3 ; xx [ 105 ] = xx [ 83 ] * xx [ 102 ] ; xx
[ 106 ] = xx [ 83 ] * xx [ 103 ] ; xx [ 107 ] = xx [ 83 ] * xx [ 104 ] ;
pm_math_Vector3_cross_ra ( xx + 102 , xx + 105 , xx + 108 ) ;
pm_math_Quaternion_xform_ra ( xx + 86 , xx + 108 , xx + 105 ) ; xx [ 108 ] =
xx [ 48 ] + xx [ 46 ] * state [ 10 ] ; xx [ 109 ] = xx [ 59 ] * state [ 10 ]
- xx [ 68 ] ; xx [ 110 ] = - ( xx [ 80 ] + xx [ 60 ] * state [ 10 ] ) ; xx [
83 ] = 0.017 ; pm_math_Vector3_cross_ra ( xx + 77 , xx + 108 , xx + 111 ) ;
pm_math_Vector3_cross_ra ( xx + 77 , xx + 111 , xx + 114 ) ;
pm_math_Quaternion_inverseXform_ra ( xx + 86 , xx + 114 , xx + 111 ) ; xx [
96 ] = 1.414213562373095 ; xx [ 103 ] = 0.7071067811865476 ; xx [ 114 ] = xx
[ 96 ] * xx [ 103 ] * input [ 7 ] ; xx [ 115 ] = xx [ 83 ] * xx [ 112 ] - xx
[ 114 ] ; xx [ 116 ] = 5.0 ; xx [ 117 ] = ( input [ 1 ] - state [ 10 ] - xx [
116 ] * state [ 11 ] - xx [ 115 ] ) / xx [ 83 ] ; xx [ 118 ] = xx [ 83 ] * (
xx [ 111 ] - xx [ 90 ] * xx [ 104 ] * state [ 11 ] ) ; xx [ 119 ] = xx [ 115
] + xx [ 83 ] * xx [ 117 ] ; xx [ 120 ] = ( xx [ 90 ] * xx [ 102 ] * state [
11 ] + xx [ 113 ] ) * xx [ 83 ] - ( input [ 7 ] - xx [ 114 ] ) ;
pm_math_Quaternion_xform_ra ( xx + 86 , xx + 118 , xx + 111 ) ;
pm_math_Vector3_cross_ra ( xx + 108 , xx + 111 , xx + 118 ) ; xx [ 121 ] = xx
[ 81 ] ; xx [ 122 ] = - 7.396429119177483e-8 ; xx [ 123 ] = xx [ 82 ] ; xx [
124 ] = 4.167847961603077e-6 ; pm_math_Quaternion_inverseXform_ra ( xx + 121
, xx + 77 , xx + 125 ) ; xx [ 81 ] = 1.0e-6 ; xx [ 128 ] = xx [ 81 ] * xx [
125 ] ; xx [ 129 ] = xx [ 81 ] * xx [ 126 ] ; xx [ 130 ] = xx [ 81 ] * xx [
127 ] ; pm_math_Vector3_cross_ra ( xx + 125 , xx + 128 , xx + 131 ) ;
pm_math_Quaternion_xform_ra ( xx + 121 , xx + 131 , xx + 128 ) ; xx [ 131 ] =
xx [ 48 ] - 8.310758753113379e-6 * state [ 8 ] ; xx [ 132 ] = - ( xx [ 68 ] +
xx [ 59 ] * state [ 8 ] ) ; xx [ 133 ] = 6.610584966093569e-7 * state [ 8 ] -
xx [ 80 ] ; pm_math_Vector3_cross_ra ( xx + 77 , xx + 131 , xx + 80 ) ;
pm_math_Vector3_cross_ra ( xx + 77 , xx + 80 , xx + 134 ) ;
pm_math_Quaternion_inverseXform_ra ( xx + 121 , xx + 134 , xx + 77 ) ; xx [
48 ] = xx [ 96 ] * xx [ 103 ] * input [ 9 ] ; xx [ 68 ] = xx [ 48 ] + xx [ 83
] * xx [ 78 ] ; xx [ 80 ] = ( input [ 0 ] - state [ 8 ] - xx [ 116 ] * state
[ 9 ] + xx [ 68 ] ) / xx [ 83 ] ; xx [ 102 ] = ( xx [ 90 ] * xx [ 127 ] *
state [ 9 ] + xx [ 77 ] ) * xx [ 83 ] ; xx [ 103 ] = xx [ 68 ] - xx [ 83 ] *
xx [ 80 ] ; xx [ 104 ] = xx [ 83 ] * ( xx [ 79 ] - xx [ 90 ] * xx [ 125 ] *
state [ 9 ] ) - ( input [ 9 ] - xx [ 48 ] ) ; pm_math_Quaternion_xform_ra (
xx + 121 , xx + 102 , xx + 77 ) ; pm_math_Vector3_cross_ra ( xx + 131 , xx +
77 , xx + 81 ) ; xx [ 48 ] = 0.01699999999882584 ; xx [ 68 ] =
9.33970782657223e-14 ; xx [ 96 ] = 1.123799445909647e-8 ; xx [ 102 ] =
0.01699999999999258 ; xx [ 134 ] = xx [ 48 ] ; xx [ 135 ] = -
1.412828987945046e-7 ; xx [ 136 ] = xx [ 68 ] ; xx [ 137 ] = -
1.412828987945046e-7 ; xx [ 138 ] = 1.181597059566887e-12 ; xx [ 139 ] = xx [
96 ] ; xx [ 140 ] = xx [ 68 ] ; xx [ 141 ] = xx [ 96 ] ; xx [ 142 ] = xx [
102 ] ; pm_math_Matrix3x3_postCross_ra ( xx + 134 , xx + 108 , xx + 143 ) ;
pm_math_Matrix3x3_preCross_ra ( xx + 143 , xx + 108 , xx + 134 ) ; xx [ 96 ]
= 1.0e-6 ; xx [ 103 ] = - 1.412828987980302e-7 ; xx [ 152 ] = xx [ 48 ] ; xx
[ 153 ] = xx [ 103 ] ; xx [ 154 ] = xx [ 68 ] ; xx [ 155 ] = xx [ 103 ] ; xx
[ 156 ] = 1.181597059603377e-12 ; xx [ 157 ] = 1.123799444237311e-8 ; xx [
158 ] = xx [ 68 ] ; xx [ 159 ] = 1.123799444237312e-8 ; xx [ 160 ] = xx [ 102
] ; pm_math_Matrix3x3_postCross_ra ( xx + 152 , xx + 131 , xx + 161 ) ;
pm_math_Matrix3x3_preCross_ra ( xx + 161 , xx + 131 , xx + 152 ) ; xx [ 48 ]
= 1.080949177762983e-3 - xx [ 134 ] + xx [ 96 ] - xx [ 152 ] ; xx [ 68 ] =
2.740035779081904e-24 - xx [ 135 ] + 7.573064690121713e-29 - xx [ 153 ] ; xx
[ 102 ] = 2.646977960169689e-23 ; xx [ 103 ] = 5.421010862427522e-20 - xx [
136 ] + xx [ 102 ] - xx [ 154 ] ; xx [ 104 ] = 1.085674553975849e-24 - xx [
137 ] + 1.691317780793849e-27 - xx [ 155 ] ; xx [ 114 ] =
1.367360393682158e-3 - xx [ 138 ] + xx [ 96 ] - xx [ 156 ] ; xx [ 96 ] =
4.135903062765138e-25 + xx [ 139 ] + 4.03896783473158e-28 + xx [ 157 ] ; xx [
115 ] = 2.710505431213761e-20 - xx [ 140 ] + xx [ 102 ] - xx [ 158 ] ; xx [
102 ] = 2.067951531382569e-25 + xx [ 141 ] + 2.01948391736579e-28 + xx [ 159
] ; xx [ 116 ] = 1.384428850918516e-3 - xx [ 142 ] + 1.000000000000001e-6 -
xx [ 160 ] ; xx [ 134 ] = xx [ 48 ] ; xx [ 135 ] = xx [ 68 ] ; xx [ 136 ] =
xx [ 103 ] ; xx [ 137 ] = xx [ 104 ] ; xx [ 138 ] = xx [ 114 ] ; xx [ 139 ] =
- xx [ 96 ] ; xx [ 140 ] = xx [ 115 ] ; xx [ 141 ] = - xx [ 102 ] ; xx [ 142
] = xx [ 116 ] ; xx [ 125 ] = xx [ 26 ] ; xx [ 126 ] = xx [ 61 ] ; xx [ 127 ]
= - xx [ 69 ] ; pm_math_Vector3_cross_ra ( xx + 65 , xx + 125 , xx + 152 ) ;
pm_math_Matrix3x3_xform_ra ( xx + 134 , xx + 152 , xx + 65 ) ; xx [ 26 ] = xx
[ 143 ] + xx [ 161 ] ; xx [ 61 ] = xx [ 146 ] + xx [ 164 ] ; xx [ 69 ] = xx [
149 ] + xx [ 167 ] ; xx [ 125 ] = xx [ 144 ] + xx [ 162 ] ; xx [ 126 ] = xx [
147 ] + xx [ 165 ] ; xx [ 127 ] = xx [ 150 ] + xx [ 168 ] ; xx [ 143 ] = xx [
145 ] + xx [ 163 ] ; xx [ 144 ] = xx [ 148 ] + xx [ 166 ] ; xx [ 145 ] = xx [
151 ] + xx [ 169 ] ; xx [ 155 ] = - xx [ 26 ] ; xx [ 156 ] = - xx [ 61 ] ; xx
[ 157 ] = - xx [ 69 ] ; xx [ 158 ] = - xx [ 125 ] ; xx [ 159 ] = - xx [ 126 ]
; xx [ 160 ] = - xx [ 127 ] ; xx [ 161 ] = - xx [ 143 ] ; xx [ 162 ] = - xx [
144 ] ; xx [ 163 ] = - xx [ 145 ] ; xx [ 146 ] = 0.1239983924299681 ; xx [
147 ] = - 5.656415752171374e-4 ; xx [ 148 ] = - 2.805786735064276e-4 ;
pm_math_Vector3_cross_ra ( xx + 55 , xx + 146 , xx + 149 ) ;
pm_math_Vector3_cross_ra ( xx + 55 , xx + 149 , xx + 164 ) ;
pm_math_Quaternion_inverseXform_ra ( xx + 73 , xx + 164 , xx + 55 ) ;
pm_math_Matrix3x3_xform_ra ( xx + 155 , xx + 55 , xx + 149 ) ; xx [ 164 ] =
xx [ 70 ] - xx [ 93 ] - xx [ 99 ] + xx [ 105 ] + xx [ 118 ] + xx [ 128 ] + xx
[ 81 ] + xx [ 65 ] + xx [ 149 ] ; xx [ 165 ] = xx [ 46 ] ; xx [ 166 ] = xx [
59 ] ; xx [ 167 ] = - xx [ 60 ] ; xx [ 168 ] = xx [ 71 ] - xx [ 94 ] - xx [
100 ] + xx [ 106 ] + xx [ 119 ] + xx [ 129 ] + xx [ 82 ] + xx [ 66 ] + xx [
150 ] ; xx [ 65 ] = xx [ 72 ] - xx [ 95 ] - xx [ 101 ] + xx [ 107 ] + xx [
120 ] + xx [ 130 ] + xx [ 83 ] + xx [ 67 ] + xx [ 151 ] ; xx [ 70 ] = xx [
164 ] ; xx [ 71 ] = xx [ 168 ] ; xx [ 72 ] = xx [ 65 ] ;
pm_math_Matrix3x3_xform_ra ( xx + 134 , xx + 165 , xx + 81 ) ; xx [ 66 ] =
pm_math_Vector3_dot_ra ( xx + 165 , xx + 81 ) ; ii [ 0 ] =
factorSymmetricPosDef ( xx + 66 , 1 , xx + 67 ) ; if ( ii [ 0 ] != 0 ) {
return sm_ssci_recordRunTimeError (
"sm:compiler:messages:simulationErrors:DegenerateMass" ,
 "'openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/joint4' has a degenerate mass distribution on its follower side."
, neDiagMgr ) ; } xx [ 67 ] = ( input [ 5 ] - xx [ 1 ] * state [ 7 ] -
pm_math_Vector3_dot_ra ( xx + 165 , xx + 70 ) ) / xx [ 66 ] ; xx [ 70 ] = xx
[ 164 ] + xx [ 67 ] * xx [ 81 ] ; xx [ 71 ] = xx [ 168 ] + xx [ 67 ] * xx [
82 ] ; xx [ 72 ] = xx [ 65 ] + xx [ 67 ] * xx [ 83 ] ;
pm_math_Quaternion_xform_ra ( xx + 73 , xx + 70 , xx + 93 ) ;
pm_math_Matrix3x3_transposeXform_ra ( xx + 155 , xx + 152 , xx + 70 ) ; xx [
65 ] = 0.1749999999976517 ; xx [ 99 ] = 2.825657975925348e-7 ; xx [ 100 ] = -
xx [ 99 ] ; xx [ 101 ] = 1.867941565314446e-13 ; xx [ 105 ] =
0.1410000000023632 ; xx [ 106 ] = 2.247598890146958e-8 ; xx [ 107 ] =
2.247598890146959e-8 ; xx [ 118 ] = 0.1749999999999851 ; xx [ 134 ] = xx [ 65
] ; xx [ 135 ] = xx [ 100 ] ; xx [ 136 ] = xx [ 101 ] ; xx [ 137 ] = xx [ 100
] ; xx [ 138 ] = xx [ 105 ] ; xx [ 139 ] = xx [ 106 ] ; xx [ 140 ] = xx [ 101
] ; xx [ 141 ] = xx [ 107 ] ; xx [ 142 ] = xx [ 118 ] ;
pm_math_Matrix3x3_xform_ra ( xx + 134 , xx + 55 , xx + 128 ) ;
pm_math_Matrix3x3_transposeXform_ra ( xx + 155 , xx + 165 , xx + 134 ) ; xx [
137 ] = xx [ 111 ] - ( xx [ 91 ] + xx [ 97 ] ) + xx [ 77 ] + xx [ 70 ] + xx [
128 ] + xx [ 67 ] * xx [ 134 ] ; xx [ 138 ] = xx [ 112 ] - ( xx [ 92 ] - xx [
98 ] ) + xx [ 78 ] + xx [ 71 ] + xx [ 129 ] + xx [ 67 ] * xx [ 135 ] ; xx [
139 ] = xx [ 113 ] - ( xx [ 84 ] + xx [ 85 ] ) + xx [ 79 ] + xx [ 72 ] + xx [
130 ] + xx [ 67 ] * xx [ 136 ] ; pm_math_Quaternion_xform_ra ( xx + 73 , xx +
137 , xx + 70 ) ; pm_math_Vector3_cross_ra ( xx + 146 , xx + 70 , xx + 77 ) ;
xx [ 84 ] = xx [ 73 ] * xx [ 73 ] ; xx [ 85 ] = 1.0 ; xx [ 91 ] = xx [ 74 ] *
xx [ 75 ] ; xx [ 92 ] = xx [ 73 ] * xx [ 76 ] ; xx [ 97 ] = xx [ 74 ] * xx [
76 ] ; xx [ 98 ] = xx [ 73 ] * xx [ 75 ] ; xx [ 100 ] = xx [ 75 ] * xx [ 76 ]
; xx [ 111 ] = xx [ 73 ] * xx [ 74 ] ; xx [ 155 ] = ( xx [ 84 ] + xx [ 74 ] *
xx [ 74 ] ) * xx [ 90 ] - xx [ 85 ] ; xx [ 156 ] = xx [ 90 ] * ( xx [ 91 ] -
xx [ 92 ] ) ; xx [ 157 ] = ( xx [ 97 ] + xx [ 98 ] ) * xx [ 90 ] ; xx [ 158 ]
= ( xx [ 91 ] + xx [ 92 ] ) * xx [ 90 ] ; xx [ 159 ] = ( xx [ 84 ] + xx [ 75
] * xx [ 75 ] ) * xx [ 90 ] - xx [ 85 ] ; xx [ 160 ] = xx [ 90 ] * ( xx [ 100
] - xx [ 111 ] ) ; xx [ 161 ] = xx [ 90 ] * ( xx [ 97 ] - xx [ 98 ] ) ; xx [
162 ] = ( xx [ 100 ] + xx [ 111 ] ) * xx [ 90 ] ; xx [ 163 ] = ( xx [ 84 ] +
xx [ 76 ] * xx [ 76 ] ) * xx [ 90 ] - xx [ 85 ] ; xx [ 84 ] = xx [ 81 ] / xx
[ 66 ] ; xx [ 91 ] = xx [ 84 ] * xx [ 82 ] ; xx [ 92 ] = xx [ 84 ] * xx [ 83
] ; xx [ 97 ] = xx [ 82 ] / xx [ 66 ] ; xx [ 98 ] = xx [ 97 ] * xx [ 83 ] ;
xx [ 100 ] = xx [ 83 ] / xx [ 66 ] ; xx [ 164 ] = xx [ 48 ] - xx [ 84 ] * xx
[ 81 ] ; xx [ 165 ] = xx [ 68 ] - xx [ 91 ] ; xx [ 166 ] = xx [ 103 ] - xx [
92 ] ; xx [ 167 ] = xx [ 104 ] - xx [ 91 ] ; xx [ 168 ] = xx [ 114 ] - xx [
97 ] * xx [ 82 ] ; xx [ 169 ] = - ( xx [ 96 ] + xx [ 98 ] ) ; xx [ 170 ] = xx
[ 115 ] - xx [ 92 ] ; xx [ 171 ] = - ( xx [ 102 ] + xx [ 98 ] ) ; xx [ 172 ]
= xx [ 116 ] - xx [ 100 ] * xx [ 83 ] ; pm_math_Matrix3x3_composeTranspose_ra
( xx + 164 , xx + 155 , xx + 173 ) ; pm_math_Matrix3x3_compose_ra ( xx + 155
, xx + 173 , xx + 164 ) ; xx [ 48 ] = xx [ 134 ] / xx [ 66 ] ; xx [ 68 ] = xx
[ 135 ] / xx [ 66 ] ; xx [ 91 ] = xx [ 136 ] / xx [ 66 ] ; xx [ 173 ] = - (
xx [ 26 ] + xx [ 48 ] * xx [ 81 ] ) ; xx [ 174 ] = - ( xx [ 61 ] + xx [ 68 ]
* xx [ 81 ] ) ; xx [ 175 ] = - ( xx [ 69 ] + xx [ 91 ] * xx [ 81 ] ) ; xx [
176 ] = - ( xx [ 125 ] + xx [ 48 ] * xx [ 82 ] ) ; xx [ 177 ] = - ( xx [ 126
] + xx [ 68 ] * xx [ 82 ] ) ; xx [ 178 ] = - ( xx [ 127 ] + xx [ 91 ] * xx [
82 ] ) ; xx [ 179 ] = - ( xx [ 143 ] + xx [ 48 ] * xx [ 83 ] ) ; xx [ 180 ] =
- ( xx [ 144 ] + xx [ 68 ] * xx [ 83 ] ) ; xx [ 181 ] = - ( xx [ 145 ] + xx [
91 ] * xx [ 83 ] ) ; pm_math_Matrix3x3_composeTranspose_ra ( xx + 173 , xx +
155 , xx + 137 ) ; pm_math_Matrix3x3_compose_ra ( xx + 155 , xx + 137 , xx +
173 ) ; pm_math_Matrix3x3_postCross_ra ( xx + 173 , xx + 146 , xx + 137 ) ;
xx [ 26 ] = - ( xx [ 99 ] + xx [ 48 ] * xx [ 135 ] ) ; xx [ 61 ] = xx [ 101 ]
- xx [ 48 ] * xx [ 136 ] ; xx [ 66 ] = xx [ 68 ] * xx [ 136 ] ; xx [ 182 ] =
xx [ 65 ] - xx [ 48 ] * xx [ 134 ] ; xx [ 183 ] = xx [ 26 ] ; xx [ 184 ] = xx
[ 61 ] ; xx [ 185 ] = xx [ 26 ] ; xx [ 186 ] = xx [ 105 ] - xx [ 68 ] * xx [
135 ] ; xx [ 187 ] = xx [ 106 ] - xx [ 66 ] ; xx [ 188 ] = xx [ 61 ] ; xx [
189 ] = xx [ 107 ] - xx [ 66 ] ; xx [ 190 ] = xx [ 118 ] - xx [ 91 ] * xx [
136 ] ; pm_math_Matrix3x3_composeTranspose_ra ( xx + 182 , xx + 155 , xx +
191 ) ; pm_math_Matrix3x3_compose_ra ( xx + 155 , xx + 191 , xx + 182 ) ;
pm_math_Matrix3x3_postCross_ra ( xx + 182 , xx + 146 , xx + 155 ) ;
pm_math_Matrix3x3_preCross_ra ( xx + 155 , xx + 146 , xx + 191 ) ; xx [ 26 ]
= xx [ 164 ] - xx [ 137 ] - xx [ 137 ] - xx [ 191 ] ; xx [ 61 ] = xx [ 165 ]
- xx [ 138 ] - xx [ 140 ] - xx [ 192 ] ; xx [ 65 ] = xx [ 166 ] - xx [ 139 ]
- xx [ 143 ] - xx [ 193 ] ; xx [ 66 ] = xx [ 167 ] - xx [ 140 ] - xx [ 138 ]
- xx [ 194 ] ; xx [ 69 ] = xx [ 168 ] - xx [ 141 ] - xx [ 141 ] - xx [ 195 ]
; xx [ 81 ] = xx [ 169 ] - xx [ 142 ] - xx [ 144 ] - xx [ 196 ] ; xx [ 82 ] =
xx [ 170 ] - xx [ 143 ] - xx [ 139 ] - xx [ 197 ] ; xx [ 83 ] = xx [ 171 ] -
xx [ 144 ] - xx [ 142 ] - xx [ 198 ] ; xx [ 92 ] = xx [ 172 ] - xx [ 145 ] -
xx [ 145 ] - xx [ 199 ] ; xx [ 134 ] = xx [ 49 ] + xx [ 26 ] ; xx [ 135 ] =
xx [ 61 ] ; xx [ 136 ] = xx [ 65 ] ; xx [ 137 ] = xx [ 66 ] ; xx [ 138 ] = xx
[ 50 ] + xx [ 69 ] ; xx [ 139 ] = xx [ 81 ] ; xx [ 140 ] = xx [ 82 ] ; xx [
141 ] = xx [ 83 ] ; xx [ 142 ] = xx [ 58 ] + xx [ 92 ] ; xx [ 101 ] = xx [ 20
] ; xx [ 102 ] = xx [ 39 ] ; xx [ 103 ] = xx [ 47 ] ;
pm_math_Vector3_cross_ra ( xx + 43 , xx + 101 , xx + 104 ) ;
pm_math_Matrix3x3_xform_ra ( xx + 134 , xx + 104 , xx + 43 ) ; xx [ 20 ] = xx
[ 173 ] - xx [ 155 ] ; xx [ 39 ] = xx [ 174 ] - xx [ 158 ] ; xx [ 47 ] = xx [
175 ] - xx [ 161 ] ; xx [ 96 ] = xx [ 176 ] - xx [ 156 ] ; xx [ 98 ] = xx [
177 ] - xx [ 159 ] ; xx [ 99 ] = xx [ 178 ] - xx [ 162 ] ; xx [ 101 ] = xx [
179 ] - xx [ 157 ] ; xx [ 102 ] = xx [ 180 ] - xx [ 160 ] ; xx [ 103 ] = xx [
181 ] - xx [ 163 ] ; xx [ 155 ] = xx [ 20 ] ; xx [ 156 ] = xx [ 39 ] ; xx [
157 ] = xx [ 47 ] ; xx [ 158 ] = xx [ 96 ] ; xx [ 159 ] = xx [ 98 ] ; xx [
160 ] = xx [ 99 ] ; xx [ 161 ] = xx [ 101 ] ; xx [ 162 ] = xx [ 102 ] ; xx [
163 ] = xx [ 103 ] ; xx [ 111 ] = 0.01017880053681747 ; xx [ 112 ] = -
3.736067231021393e-4 ; xx [ 113 ] = 0.1298316311137164 ;
pm_math_Vector3_cross_ra ( xx + 32 , xx + 111 , xx + 114 ) ;
pm_math_Vector3_cross_ra ( xx + 32 , xx + 114 , xx + 118 ) ;
pm_math_Quaternion_inverseXform_ra ( xx + 51 , xx + 118 , xx + 32 ) ;
pm_math_Matrix3x3_xform_ra ( xx + 155 , xx + 32 , xx + 114 ) ; xx [ 107 ] =
xx [ 62 ] + xx [ 93 ] + xx [ 77 ] + xx [ 43 ] + xx [ 114 ] ; xx [ 118 ] = xx
[ 24 ] ; xx [ 119 ] = xx [ 37 ] ; xx [ 120 ] = xx [ 38 ] ; xx [ 125 ] = xx [
63 ] + xx [ 94 ] + xx [ 78 ] + xx [ 44 ] + xx [ 115 ] ; xx [ 43 ] = xx [ 64 ]
+ xx [ 95 ] + xx [ 79 ] + xx [ 45 ] + xx [ 116 ] ; xx [ 62 ] = xx [ 107 ] ;
xx [ 63 ] = xx [ 125 ] ; xx [ 64 ] = xx [ 43 ] ; pm_math_Matrix3x3_xform_ra (
xx + 134 , xx + 118 , xx + 77 ) ; xx [ 44 ] = pm_math_Vector3_dot_ra ( xx +
118 , xx + 77 ) ; ii [ 0 ] = factorSymmetricPosDef ( xx + 44 , 1 , xx + 45 )
; if ( ii [ 0 ] != 0 ) { return sm_ssci_recordRunTimeError (
"sm:compiler:messages:simulationErrors:DegenerateMass" ,
 "'openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/joint3' has a degenerate mass distribution on its follower side."
, neDiagMgr ) ; } xx [ 45 ] = ( input [ 4 ] - xx [ 1 ] * state [ 5 ] -
pm_math_Vector3_dot_ra ( xx + 118 , xx + 62 ) ) / xx [ 44 ] ; xx [ 62 ] = xx
[ 107 ] + xx [ 45 ] * xx [ 77 ] ; xx [ 63 ] = xx [ 125 ] + xx [ 45 ] * xx [
78 ] ; xx [ 64 ] = xx [ 43 ] + xx [ 45 ] * xx [ 79 ] ;
pm_math_Quaternion_xform_ra ( xx + 51 , xx + 62 , xx + 93 ) ;
pm_math_Matrix3x3_transposeXform_ra ( xx + 155 , xx + 104 , xx + 62 ) ; xx [
43 ] = 0.131 ; xx [ 134 ] = xx [ 43 ] + xx [ 182 ] ; xx [ 135 ] = xx [ 183 ]
; xx [ 136 ] = xx [ 184 ] ; xx [ 137 ] = xx [ 185 ] ; xx [ 138 ] = xx [ 43 ]
+ xx [ 186 ] ; xx [ 139 ] = xx [ 187 ] ; xx [ 140 ] = xx [ 188 ] ; xx [ 141 ]
= xx [ 189 ] ; xx [ 142 ] = xx [ 43 ] + xx [ 190 ] ;
pm_math_Matrix3x3_xform_ra ( xx + 134 , xx + 32 , xx + 114 ) ;
pm_math_Matrix3x3_transposeXform_ra ( xx + 155 , xx + 118 , xx + 125 ) ; xx [
118 ] = xx [ 70 ] + xx [ 62 ] + xx [ 114 ] + xx [ 45 ] * xx [ 125 ] ; xx [
119 ] = xx [ 71 ] + xx [ 63 ] + xx [ 115 ] + xx [ 45 ] * xx [ 126 ] ; xx [
120 ] = xx [ 72 ] + xx [ 64 ] + xx [ 116 ] + xx [ 45 ] * xx [ 127 ] ;
pm_math_Quaternion_xform_ra ( xx + 51 , xx + 118 , xx + 62 ) ;
pm_math_Vector3_cross_ra ( xx + 111 , xx + 62 , xx + 70 ) ; xx [ 107 ] = xx [
51 ] * xx [ 51 ] ; xx [ 114 ] = xx [ 52 ] * xx [ 53 ] ; xx [ 115 ] = xx [ 51
] * xx [ 54 ] ; xx [ 116 ] = xx [ 52 ] * xx [ 54 ] ; xx [ 118 ] = xx [ 51 ] *
xx [ 53 ] ; xx [ 119 ] = xx [ 53 ] * xx [ 54 ] ; xx [ 120 ] = xx [ 51 ] * xx
[ 52 ] ; xx [ 134 ] = ( xx [ 107 ] + xx [ 52 ] * xx [ 52 ] ) * xx [ 90 ] - xx
[ 85 ] ; xx [ 135 ] = xx [ 90 ] * ( xx [ 114 ] - xx [ 115 ] ) ; xx [ 136 ] =
( xx [ 116 ] + xx [ 118 ] ) * xx [ 90 ] ; xx [ 137 ] = ( xx [ 114 ] + xx [
115 ] ) * xx [ 90 ] ; xx [ 138 ] = ( xx [ 107 ] + xx [ 53 ] * xx [ 53 ] ) *
xx [ 90 ] - xx [ 85 ] ; xx [ 139 ] = xx [ 90 ] * ( xx [ 119 ] - xx [ 120 ] )
; xx [ 140 ] = xx [ 90 ] * ( xx [ 116 ] - xx [ 118 ] ) ; xx [ 141 ] = ( xx [
119 ] + xx [ 120 ] ) * xx [ 90 ] ; xx [ 142 ] = ( xx [ 107 ] + xx [ 54 ] * xx
[ 54 ] ) * xx [ 90 ] - xx [ 85 ] ; xx [ 107 ] = xx [ 77 ] / xx [ 44 ] ; xx [
114 ] = xx [ 107 ] * xx [ 78 ] ; xx [ 115 ] = xx [ 107 ] * xx [ 79 ] ; xx [
116 ] = xx [ 78 ] / xx [ 44 ] ; xx [ 118 ] = xx [ 116 ] * xx [ 79 ] ; xx [
119 ] = xx [ 79 ] / xx [ 44 ] ; xx [ 155 ] = xx [ 26 ] - xx [ 107 ] * xx [ 77
] + xx [ 49 ] ; xx [ 156 ] = xx [ 61 ] - xx [ 114 ] ; xx [ 157 ] = xx [ 65 ]
- xx [ 115 ] ; xx [ 158 ] = xx [ 66 ] - xx [ 114 ] ; xx [ 159 ] = xx [ 69 ] -
xx [ 116 ] * xx [ 78 ] + xx [ 50 ] ; xx [ 160 ] = xx [ 81 ] - xx [ 118 ] ; xx
[ 161 ] = xx [ 82 ] - xx [ 115 ] ; xx [ 162 ] = xx [ 83 ] - xx [ 118 ] ; xx [
163 ] = xx [ 92 ] - xx [ 119 ] * xx [ 79 ] + xx [ 58 ] ;
pm_math_Matrix3x3_composeTranspose_ra ( xx + 155 , xx + 134 , xx + 164 ) ;
pm_math_Matrix3x3_compose_ra ( xx + 134 , xx + 164 , xx + 155 ) ; xx [ 26 ] =
xx [ 125 ] / xx [ 44 ] ; xx [ 49 ] = xx [ 126 ] / xx [ 44 ] ; xx [ 50 ] = xx
[ 127 ] / xx [ 44 ] ; xx [ 164 ] = xx [ 20 ] - xx [ 26 ] * xx [ 77 ] ; xx [
165 ] = xx [ 39 ] - xx [ 49 ] * xx [ 77 ] ; xx [ 166 ] = xx [ 47 ] - xx [ 50
] * xx [ 77 ] ; xx [ 167 ] = xx [ 96 ] - xx [ 26 ] * xx [ 78 ] ; xx [ 168 ] =
xx [ 98 ] - xx [ 49 ] * xx [ 78 ] ; xx [ 169 ] = xx [ 99 ] - xx [ 50 ] * xx [
78 ] ; xx [ 170 ] = xx [ 101 ] - xx [ 26 ] * xx [ 79 ] ; xx [ 171 ] = xx [
102 ] - xx [ 49 ] * xx [ 79 ] ; xx [ 172 ] = xx [ 103 ] - xx [ 50 ] * xx [ 79
] ; pm_math_Matrix3x3_composeTranspose_ra ( xx + 164 , xx + 134 , xx + 173 )
; pm_math_Matrix3x3_compose_ra ( xx + 134 , xx + 173 , xx + 164 ) ;
pm_math_Matrix3x3_postCross_ra ( xx + 164 , xx + 111 , xx + 173 ) ; xx [ 20 ]
= xx [ 26 ] * xx [ 126 ] ; xx [ 39 ] = xx [ 26 ] * xx [ 127 ] ; xx [ 44 ] =
xx [ 49 ] * xx [ 127 ] ; xx [ 191 ] = xx [ 182 ] - xx [ 26 ] * xx [ 125 ] +
xx [ 43 ] ; xx [ 192 ] = xx [ 183 ] - xx [ 20 ] ; xx [ 193 ] = xx [ 184 ] -
xx [ 39 ] ; xx [ 194 ] = xx [ 185 ] - xx [ 20 ] ; xx [ 195 ] = xx [ 186 ] -
xx [ 49 ] * xx [ 126 ] + xx [ 43 ] ; xx [ 196 ] = xx [ 187 ] - xx [ 44 ] ; xx
[ 197 ] = xx [ 188 ] - xx [ 39 ] ; xx [ 198 ] = xx [ 189 ] - xx [ 44 ] ; xx [
199 ] = xx [ 190 ] - xx [ 50 ] * xx [ 127 ] + xx [ 43 ] ;
pm_math_Matrix3x3_composeTranspose_ra ( xx + 191 , xx + 134 , xx + 182 ) ;
pm_math_Matrix3x3_compose_ra ( xx + 134 , xx + 182 , xx + 191 ) ;
pm_math_Matrix3x3_postCross_ra ( xx + 191 , xx + 111 , xx + 134 ) ;
pm_math_Matrix3x3_preCross_ra ( xx + 134 , xx + 111 , xx + 182 ) ; xx [ 20 ]
= xx [ 155 ] - xx [ 173 ] - xx [ 173 ] - xx [ 182 ] ; xx [ 39 ] = xx [ 156 ]
- xx [ 174 ] - xx [ 176 ] - xx [ 183 ] ; xx [ 43 ] = xx [ 157 ] - xx [ 175 ]
- xx [ 179 ] - xx [ 184 ] ; xx [ 44 ] = xx [ 158 ] - xx [ 176 ] - xx [ 174 ]
- xx [ 185 ] ; xx [ 47 ] = xx [ 159 ] - xx [ 177 ] - xx [ 177 ] - xx [ 186 ]
; xx [ 58 ] = xx [ 160 ] - xx [ 178 ] - xx [ 180 ] - xx [ 187 ] ; xx [ 61 ] =
xx [ 161 ] - xx [ 179 ] - xx [ 175 ] - xx [ 188 ] ; xx [ 65 ] = xx [ 162 ] -
xx [ 180 ] - xx [ 178 ] - xx [ 189 ] ; xx [ 66 ] = xx [ 163 ] - xx [ 181 ] -
xx [ 181 ] - xx [ 190 ] ; xx [ 155 ] = xx [ 27 ] + xx [ 20 ] ; xx [ 156 ] =
xx [ 39 ] ; xx [ 157 ] = xx [ 43 ] ; xx [ 158 ] = xx [ 44 ] ; xx [ 159 ] = xx
[ 35 ] + xx [ 47 ] ; xx [ 160 ] = xx [ 58 ] ; xx [ 161 ] = xx [ 61 ] ; xx [
162 ] = xx [ 65 ] ; xx [ 163 ] = xx [ 36 ] + xx [ 66 ] ; xx [ 77 ] = - xx [
12 ] ; xx [ 78 ] = xx [ 21 ] ; xx [ 79 ] = xx [ 25 ] ;
pm_math_Vector3_cross_ra ( xx + 17 , xx + 77 , xx + 81 ) ;
pm_math_Matrix3x3_xform_ra ( xx + 155 , xx + 81 , xx + 17 ) ; xx [ 12 ] = xx
[ 164 ] - xx [ 134 ] ; xx [ 21 ] = xx [ 165 ] - xx [ 137 ] ; xx [ 25 ] = xx [
166 ] - xx [ 140 ] ; xx [ 69 ] = xx [ 167 ] - xx [ 135 ] ; xx [ 77 ] = xx [
168 ] - xx [ 138 ] ; xx [ 78 ] = xx [ 169 ] - xx [ 141 ] ; xx [ 79 ] = xx [
170 ] - xx [ 136 ] ; xx [ 92 ] = xx [ 171 ] - xx [ 139 ] ; xx [ 96 ] = xx [
172 ] - xx [ 142 ] ; xx [ 134 ] = xx [ 12 ] ; xx [ 135 ] = xx [ 21 ] ; xx [
136 ] = xx [ 25 ] ; xx [ 137 ] = xx [ 69 ] ; xx [ 138 ] = xx [ 77 ] ; xx [
139 ] = xx [ 78 ] ; xx [ 140 ] = xx [ 79 ] ; xx [ 141 ] = xx [ 92 ] ; xx [
142 ] = xx [ 96 ] ; xx [ 101 ] = 2.543322406041548e-4 ; xx [ 102 ] = -
6.134092247471828e-4 ; xx [ 103 ] = 0.05799619853261406 ;
pm_math_Vector3_cross_ra ( xx + 8 , xx + 101 , xx + 125 ) ;
pm_math_Vector3_cross_ra ( xx + 8 , xx + 125 , xx + 128 ) ;
pm_math_Quaternion_inverseXform_ra ( xx + 28 , xx + 128 , xx + 8 ) ;
pm_math_Matrix3x3_xform_ra ( xx + 134 , xx + 8 , xx + 125 ) ; xx [ 98 ] = xx
[ 40 ] + xx [ 93 ] + xx [ 70 ] + xx [ 17 ] + xx [ 125 ] ; xx [ 128 ] = - xx [
13 ] ; xx [ 129 ] = xx [ 22 ] ; xx [ 130 ] = xx [ 23 ] ; xx [ 99 ] = xx [ 41
] + xx [ 94 ] + xx [ 71 ] + xx [ 18 ] + xx [ 126 ] ; xx [ 17 ] = xx [ 42 ] +
xx [ 95 ] + xx [ 72 ] + xx [ 19 ] + xx [ 127 ] ; xx [ 40 ] = xx [ 98 ] ; xx [
41 ] = xx [ 99 ] ; xx [ 42 ] = xx [ 17 ] ; pm_math_Matrix3x3_xform_ra ( xx +
155 , xx + 128 , xx + 70 ) ; xx [ 18 ] = pm_math_Vector3_dot_ra ( xx + 128 ,
xx + 70 ) ; ii [ 0 ] = factorSymmetricPosDef ( xx + 18 , 1 , xx + 19 ) ; if (
ii [ 0 ] != 0 ) { return sm_ssci_recordRunTimeError (
"sm:compiler:messages:simulationErrors:DegenerateMass" ,
 "'openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/joint2' has a degenerate mass distribution on its follower side."
, neDiagMgr ) ; } xx [ 19 ] = ( input [ 3 ] - xx [ 1 ] * state [ 3 ] -
pm_math_Vector3_dot_ra ( xx + 128 , xx + 40 ) ) / xx [ 18 ] ; xx [ 40 ] = xx
[ 98 ] + xx [ 19 ] * xx [ 70 ] ; xx [ 41 ] = xx [ 99 ] + xx [ 19 ] * xx [ 71
] ; xx [ 42 ] = xx [ 17 ] + xx [ 19 ] * xx [ 72 ] ;
pm_math_Quaternion_xform_ra ( xx + 28 , xx + 40 , xx + 93 ) ;
pm_math_Matrix3x3_transposeXform_ra ( xx + 134 , xx + 81 , xx + 40 ) ; xx [
17 ] = 0.136 ; xx [ 155 ] = xx [ 17 ] + xx [ 191 ] ; xx [ 156 ] = xx [ 192 ]
; xx [ 157 ] = xx [ 193 ] ; xx [ 158 ] = xx [ 194 ] ; xx [ 159 ] = xx [ 17 ]
+ xx [ 195 ] ; xx [ 160 ] = xx [ 196 ] ; xx [ 161 ] = xx [ 197 ] ; xx [ 162 ]
= xx [ 198 ] ; xx [ 163 ] = xx [ 17 ] + xx [ 199 ] ;
pm_math_Matrix3x3_xform_ra ( xx + 155 , xx + 8 , xx + 125 ) ;
pm_math_Matrix3x3_transposeXform_ra ( xx + 134 , xx + 128 , xx + 143 ) ; xx [
128 ] = xx [ 62 ] + xx [ 40 ] + xx [ 125 ] + xx [ 19 ] * xx [ 143 ] ; xx [
129 ] = xx [ 63 ] + xx [ 41 ] + xx [ 126 ] + xx [ 19 ] * xx [ 144 ] ; xx [
130 ] = xx [ 64 ] + xx [ 42 ] + xx [ 127 ] + xx [ 19 ] * xx [ 145 ] ;
pm_math_Quaternion_xform_ra ( xx + 28 , xx + 128 , xx + 40 ) ;
pm_math_Vector3_cross_ra ( xx + 101 , xx + 40 , xx + 62 ) ; xx [ 40 ] = xx [
14 ] + xx [ 93 ] + xx [ 62 ] ; xx [ 41 ] = xx [ 15 ] + xx [ 94 ] + xx [ 63 ]
; xx [ 42 ] = xx [ 16 ] + xx [ 95 ] + xx [ 64 ] ; xx [ 14 ] = xx [ 28 ] * xx
[ 28 ] ; xx [ 15 ] = xx [ 29 ] * xx [ 30 ] ; xx [ 16 ] = xx [ 28 ] * xx [ 31
] ; xx [ 62 ] = xx [ 29 ] * xx [ 31 ] ; xx [ 63 ] = xx [ 28 ] * xx [ 30 ] ;
xx [ 64 ] = xx [ 30 ] * xx [ 31 ] ; xx [ 93 ] = xx [ 28 ] * xx [ 29 ] ; xx [
134 ] = ( xx [ 14 ] + xx [ 29 ] * xx [ 29 ] ) * xx [ 90 ] - xx [ 85 ] ; xx [
135 ] = xx [ 90 ] * ( xx [ 15 ] - xx [ 16 ] ) ; xx [ 136 ] = ( xx [ 62 ] + xx
[ 63 ] ) * xx [ 90 ] ; xx [ 137 ] = ( xx [ 15 ] + xx [ 16 ] ) * xx [ 90 ] ;
xx [ 138 ] = ( xx [ 14 ] + xx [ 30 ] * xx [ 30 ] ) * xx [ 90 ] - xx [ 85 ] ;
xx [ 139 ] = xx [ 90 ] * ( xx [ 64 ] - xx [ 93 ] ) ; xx [ 140 ] = xx [ 90 ] *
( xx [ 62 ] - xx [ 63 ] ) ; xx [ 141 ] = ( xx [ 64 ] + xx [ 93 ] ) * xx [ 90
] ; xx [ 142 ] = ( xx [ 14 ] + xx [ 31 ] * xx [ 31 ] ) * xx [ 90 ] - xx [ 85
] ; xx [ 14 ] = xx [ 70 ] / xx [ 18 ] ; xx [ 15 ] = xx [ 14 ] * xx [ 71 ] ;
xx [ 16 ] = xx [ 14 ] * xx [ 72 ] ; xx [ 62 ] = xx [ 71 ] / xx [ 18 ] ; xx [
63 ] = xx [ 62 ] * xx [ 72 ] ; xx [ 64 ] = xx [ 72 ] / xx [ 18 ] ; xx [ 155 ]
= xx [ 20 ] - xx [ 14 ] * xx [ 70 ] + xx [ 27 ] ; xx [ 156 ] = xx [ 39 ] - xx
[ 15 ] ; xx [ 157 ] = xx [ 43 ] - xx [ 16 ] ; xx [ 158 ] = xx [ 44 ] - xx [
15 ] ; xx [ 159 ] = xx [ 47 ] - xx [ 62 ] * xx [ 71 ] + xx [ 35 ] ; xx [ 160
] = xx [ 58 ] - xx [ 63 ] ; xx [ 161 ] = xx [ 61 ] - xx [ 16 ] ; xx [ 162 ] =
xx [ 65 ] - xx [ 63 ] ; xx [ 163 ] = xx [ 66 ] - xx [ 64 ] * xx [ 72 ] + xx [
36 ] ; pm_math_Matrix3x3_composeTranspose_ra ( xx + 155 , xx + 134 , xx + 164
) ; pm_math_Matrix3x3_compose_ra ( xx + 134 , xx + 164 , xx + 155 ) ; xx [ 15
] = xx [ 143 ] / xx [ 18 ] ; xx [ 16 ] = xx [ 144 ] / xx [ 18 ] ; xx [ 20 ] =
xx [ 145 ] / xx [ 18 ] ; xx [ 164 ] = xx [ 12 ] - xx [ 15 ] * xx [ 70 ] ; xx
[ 165 ] = xx [ 21 ] - xx [ 16 ] * xx [ 70 ] ; xx [ 166 ] = xx [ 25 ] - xx [
20 ] * xx [ 70 ] ; xx [ 167 ] = xx [ 69 ] - xx [ 15 ] * xx [ 71 ] ; xx [ 168
] = xx [ 77 ] - xx [ 16 ] * xx [ 71 ] ; xx [ 169 ] = xx [ 78 ] - xx [ 20 ] *
xx [ 71 ] ; xx [ 170 ] = xx [ 79 ] - xx [ 15 ] * xx [ 72 ] ; xx [ 171 ] = xx
[ 92 ] - xx [ 16 ] * xx [ 72 ] ; xx [ 172 ] = xx [ 96 ] - xx [ 20 ] * xx [ 72
] ; pm_math_Matrix3x3_composeTranspose_ra ( xx + 164 , xx + 134 , xx + 173 )
; pm_math_Matrix3x3_compose_ra ( xx + 134 , xx + 173 , xx + 164 ) ;
pm_math_Matrix3x3_postCross_ra ( xx + 164 , xx + 101 , xx + 173 ) ; xx [ 12 ]
= xx [ 15 ] * xx [ 144 ] ; xx [ 18 ] = xx [ 15 ] * xx [ 145 ] ; xx [ 21 ] =
xx [ 16 ] * xx [ 145 ] ; xx [ 182 ] = xx [ 191 ] - xx [ 15 ] * xx [ 143 ] +
xx [ 17 ] ; xx [ 183 ] = xx [ 192 ] - xx [ 12 ] ; xx [ 184 ] = xx [ 193 ] -
xx [ 18 ] ; xx [ 185 ] = xx [ 194 ] - xx [ 12 ] ; xx [ 186 ] = xx [ 195 ] -
xx [ 16 ] * xx [ 144 ] + xx [ 17 ] ; xx [ 187 ] = xx [ 196 ] - xx [ 21 ] ; xx
[ 188 ] = xx [ 197 ] - xx [ 18 ] ; xx [ 189 ] = xx [ 198 ] - xx [ 21 ] ; xx [
190 ] = xx [ 199 ] - xx [ 20 ] * xx [ 145 ] + xx [ 17 ] ;
pm_math_Matrix3x3_composeTranspose_ra ( xx + 182 , xx + 134 , xx + 191 ) ;
pm_math_Matrix3x3_compose_ra ( xx + 134 , xx + 191 , xx + 182 ) ;
pm_math_Matrix3x3_postCross_ra ( xx + 182 , xx + 101 , xx + 134 ) ;
pm_math_Matrix3x3_preCross_ra ( xx + 134 , xx + 101 , xx + 182 ) ; xx [ 191 ]
= 2.559811676117321e-4 + xx [ 155 ] - xx [ 173 ] - xx [ 173 ] - xx [ 182 ] ;
xx [ 192 ] = xx [ 156 ] - xx [ 174 ] - xx [ 176 ] - xx [ 183 ] ; xx [ 193 ] =
xx [ 157 ] - xx [ 175 ] - xx [ 179 ] - xx [ 184 ] ; xx [ 194 ] = xx [ 158 ] -
xx [ 176 ] - xx [ 174 ] - xx [ 185 ] ; xx [ 195 ] = 2.541291830521279e-4 + xx
[ 159 ] - xx [ 177 ] - xx [ 177 ] - xx [ 186 ] ; xx [ 196 ] = xx [ 160 ] - xx
[ 178 ] - xx [ 180 ] - xx [ 187 ] ; xx [ 197 ] = xx [ 161 ] - xx [ 179 ] - xx
[ 175 ] - xx [ 188 ] ; xx [ 198 ] = xx [ 162 ] - xx [ 180 ] - xx [ 178 ] - xx
[ 189 ] ; xx [ 199 ] = 1.885715273225301e-5 + xx [ 163 ] - xx [ 181 ] - xx [
181 ] - xx [ 190 ] ; pm_math_Matrix3x3_xform_ra ( xx + 191 , xx + 5 , xx + 69
) ; xx [ 12 ] = pm_math_Vector3_dot_ra ( xx + 5 , xx + 69 ) ; ii [ 0 ] =
factorSymmetricPosDef ( xx + 12 , 1 , xx + 17 ) ; if ( ii [ 0 ] != 0 ) {
return sm_ssci_recordRunTimeError (
"sm:compiler:messages:simulationErrors:DegenerateMass" ,
 "'openManipulatorTrajectoryPlanning/Robot  (Torque Actuated)/joint1' has a degenerate mass distribution on its follower side."
, neDiagMgr ) ; } xx [ 155 ] = xx [ 164 ] - xx [ 134 ] ; xx [ 156 ] = xx [
165 ] - xx [ 137 ] ; xx [ 157 ] = xx [ 166 ] - xx [ 140 ] ; xx [ 158 ] = xx [
167 ] - xx [ 135 ] ; xx [ 159 ] = xx [ 168 ] - xx [ 138 ] ; xx [ 160 ] = xx [
169 ] - xx [ 141 ] ; xx [ 161 ] = xx [ 170 ] - xx [ 136 ] ; xx [ 162 ] = xx [
171 ] - xx [ 139 ] ; xx [ 163 ] = xx [ 172 ] - xx [ 142 ] ;
pm_math_Matrix3x3_transposeXform_ra ( xx + 155 , xx + 5 , xx + 69 ) ; xx [ 77
] = xx [ 69 ] / xx [ 12 ] ; xx [ 78 ] = xx [ 70 ] / xx [ 12 ] ; xx [ 79 ] =
xx [ 71 ] / xx [ 12 ] ; xx [ 69 ] = - 0.9999793598170716 ; xx [ 70 ] =
5.294470415447712e-3 ; xx [ 71 ] = 2.177120480925496e-3 ; xx [ 72 ] =
2.916962336305661e-3 ; xx [ 17 ] = xx [ 11 ] * state [ 0 ] ; xx [ 11 ] = sin
( xx [ 17 ] ) ; xx [ 92 ] = cos ( xx [ 17 ] ) ; xx [ 93 ] = xx [ 2 ] * xx [
11 ] ; xx [ 94 ] = - ( xx [ 3 ] * xx [ 11 ] ) ; xx [ 95 ] = xx [ 4 ] * xx [
11 ] ; pm_math_Quaternion_compose_ra ( xx + 69 , xx + 92 , xx + 125 ) ; xx [
11 ] = 9.806649999999999 ; xx [ 17 ] = xx [ 11 ] * xx [ 126 ] ; xx [ 18 ] =
xx [ 11 ] * xx [ 127 ] ; xx [ 21 ] = xx [ 90 ] * ( xx [ 128 ] * xx [ 17 ] -
xx [ 125 ] * xx [ 18 ] ) ; xx [ 25 ] = ( xx [ 125 ] * xx [ 17 ] + xx [ 128 ]
* xx [ 18 ] ) * xx [ 90 ] ; xx [ 27 ] = ( xx [ 126 ] * xx [ 17 ] + xx [ 127 ]
* xx [ 18 ] ) * xx [ 90 ] ; xx [ 69 ] = xx [ 21 ] ; xx [ 70 ] = xx [ 25 ] ;
xx [ 71 ] = xx [ 11 ] - xx [ 27 ] ; xx [ 17 ] = ( input [ 2 ] - xx [ 1 ] *
state [ 1 ] - pm_math_Vector3_dot_ra ( xx + 5 , xx + 40 ) ) / xx [ 12 ] -
pm_math_Vector3_dot_ra ( xx + 77 , xx + 69 ) ; xx [ 5 ] = xx [ 14 ] ; xx [ 6
] = xx [ 62 ] ; xx [ 7 ] = xx [ 64 ] ; xx [ 39 ] = xx [ 2 ] * xx [ 17 ] ; xx
[ 40 ] = - ( xx [ 3 ] * xx [ 17 ] ) ; xx [ 41 ] = xx [ 4 ] * xx [ 17 ] ;
pm_math_Quaternion_inverseXform_ra ( xx + 28 , xx + 39 , xx + 1 ) ; xx [ 42 ]
= xx [ 15 ] ; xx [ 43 ] = xx [ 16 ] ; xx [ 44 ] = xx [ 20 ] ;
pm_math_Vector3_cross_ra ( xx + 39 , xx + 101 , xx + 14 ) ; xx [ 39 ] = xx [
21 ] + xx [ 14 ] ; xx [ 40 ] = xx [ 25 ] + xx [ 15 ] ; xx [ 41 ] = xx [ 16 ]
- xx [ 27 ] + xx [ 11 ] ; pm_math_Quaternion_inverseXform_ra ( xx + 28 , xx +
39 , xx + 14 ) ; xx [ 4 ] = xx [ 19 ] - ( pm_math_Vector3_dot_ra ( xx + 5 ,
xx + 1 ) + pm_math_Vector3_dot_ra ( xx + 42 , xx + 14 ) ) ; xx [ 5 ] = xx [
107 ] ; xx [ 6 ] = xx [ 116 ] ; xx [ 7 ] = xx [ 119 ] ; xx [ 18 ] = xx [ 1 ]
- xx [ 13 ] * xx [ 4 ] + xx [ 81 ] ; xx [ 19 ] = xx [ 2 ] + xx [ 22 ] * xx [
4 ] + xx [ 82 ] ; xx [ 20 ] = xx [ 3 ] + xx [ 23 ] * xx [ 4 ] + xx [ 83 ] ;
pm_math_Quaternion_inverseXform_ra ( xx + 51 , xx + 18 , xx + 1 ) ; xx [ 11 ]
= xx [ 26 ] ; xx [ 12 ] = xx [ 49 ] ; xx [ 13 ] = xx [ 50 ] ;
pm_math_Vector3_cross_ra ( xx + 18 , xx + 111 , xx + 21 ) ; xx [ 18 ] = xx [
14 ] + xx [ 8 ] + xx [ 21 ] ; xx [ 19 ] = xx [ 15 ] + xx [ 9 ] + xx [ 22 ] ;
xx [ 20 ] = xx [ 16 ] + xx [ 10 ] + xx [ 23 ] ;
pm_math_Quaternion_inverseXform_ra ( xx + 51 , xx + 18 , xx + 8 ) ; xx [ 14 ]
= xx [ 45 ] - ( pm_math_Vector3_dot_ra ( xx + 5 , xx + 1 ) +
pm_math_Vector3_dot_ra ( xx + 11 , xx + 8 ) ) ; xx [ 5 ] = xx [ 84 ] ; xx [ 6
] = xx [ 97 ] ; xx [ 7 ] = xx [ 100 ] ; xx [ 11 ] = xx [ 1 ] + xx [ 24 ] * xx
[ 14 ] + xx [ 104 ] ; xx [ 12 ] = xx [ 2 ] + xx [ 37 ] * xx [ 14 ] + xx [ 105
] ; xx [ 13 ] = xx [ 3 ] + xx [ 38 ] * xx [ 14 ] + xx [ 106 ] ;
pm_math_Quaternion_inverseXform_ra ( xx + 73 , xx + 11 , xx + 1 ) ; xx [ 18 ]
= xx [ 48 ] ; xx [ 19 ] = xx [ 68 ] ; xx [ 20 ] = xx [ 91 ] ;
pm_math_Vector3_cross_ra ( xx + 11 , xx + 146 , xx + 21 ) ; xx [ 11 ] = xx [
8 ] + xx [ 32 ] + xx [ 21 ] ; xx [ 12 ] = xx [ 9 ] + xx [ 33 ] + xx [ 22 ] ;
xx [ 13 ] = xx [ 10 ] + xx [ 34 ] + xx [ 23 ] ;
pm_math_Quaternion_inverseXform_ra ( xx + 73 , xx + 11 , xx + 8 ) ; xx [ 11 ]
= xx [ 67 ] - ( pm_math_Vector3_dot_ra ( xx + 5 , xx + 1 ) +
pm_math_Vector3_dot_ra ( xx + 18 , xx + 8 ) ) ; xx [ 5 ] = xx [ 8 ] + xx [ 55
] ; xx [ 6 ] = xx [ 1 ] + xx [ 46 ] * xx [ 11 ] + xx [ 152 ] ; xx [ 7 ] = xx
[ 2 ] + xx [ 59 ] * xx [ 11 ] + xx [ 153 ] ; xx [ 8 ] = xx [ 3 ] - xx [ 60 ]
* xx [ 11 ] + xx [ 154 ] ; pm_math_Vector3_cross_ra ( xx + 6 , xx + 131 , xx
+ 1 ) ; xx [ 12 ] = xx [ 9 ] + xx [ 56 ] ; xx [ 9 ] = xx [ 10 ] + xx [ 57 ] ;
xx [ 18 ] = xx [ 5 ] + xx [ 1 ] ; xx [ 19 ] = xx [ 12 ] + xx [ 2 ] ; xx [ 20
] = xx [ 9 ] + xx [ 3 ] ; pm_math_Quaternion_inverseXform_ra ( xx + 121 , xx
+ 18 , xx + 1 ) ; pm_math_Vector3_cross_ra ( xx + 6 , xx + 108 , xx + 18 ) ;
xx [ 6 ] = xx [ 5 ] + xx [ 18 ] ; xx [ 7 ] = xx [ 12 ] + xx [ 19 ] ; xx [ 8 ]
= xx [ 9 ] + xx [ 20 ] ; pm_math_Quaternion_inverseXform_ra ( xx + 86 , xx +
6 , xx + 18 ) ; logVector [ 0 ] = xx [ 0 ] * state [ 0 ] ; logVector [ 1 ] =
xx [ 0 ] * state [ 1 ] ; logVector [ 2 ] = xx [ 0 ] * state [ 2 ] ; logVector
[ 3 ] = xx [ 0 ] * state [ 3 ] ; logVector [ 4 ] = xx [ 0 ] * state [ 4 ] ;
logVector [ 5 ] = xx [ 0 ] * state [ 5 ] ; logVector [ 6 ] = xx [ 0 ] * state
[ 6 ] ; logVector [ 7 ] = xx [ 0 ] * state [ 7 ] ; logVector [ 8 ] = state [
8 ] ; logVector [ 9 ] = state [ 9 ] ; logVector [ 10 ] = state [ 10 ] ;
logVector [ 11 ] = state [ 11 ] ; logVector [ 12 ] = xx [ 0 ] * xx [ 17 ] ;
logVector [ 13 ] = xx [ 0 ] * xx [ 4 ] ; logVector [ 14 ] = xx [ 0 ] * xx [
14 ] ; logVector [ 15 ] = xx [ 0 ] * xx [ 11 ] ; logVector [ 16 ] = xx [ 80 ]
+ xx [ 2 ] ; logVector [ 17 ] = xx [ 117 ] - xx [ 19 ] ; errorResult [ 0 ] =
0.0 ; return NULL ; }
